#include "WPILib.h"

class Robot : public SimpleRobot
{
	RobotDrive myRobot;
	Joystick baseStick1, baseStick2;
	
public:
	Robot(void):
		// Initialize Robot
		myRobot(1, 2, 3, 4),
		baseStick1(1),
		baseStick2(2)
		{
			myRobot.SetExpiration(0.1);
		}

	void Autonomous(void){
		// Safety Functions
		myRobot.SetSafetyEnabled(true);
	}
	
	void OperatorControl(void){
		// Safety Functions
		GetWatchdog().SetEnabled(true);
		myRobot.SetSafetyEnabled(false);
		// Initialize Arm Motors
		SafePWM armMotorTop(7);
		SafePWM armMotorBase(10);
		// Initialize Arm Joystick
		Joystick armStick(3);
		// Invert Rear Motors
		myRobot.SetInvertedMotor(myRobot.kRearLeftMotor, true);
		myRobot.SetInvertedMotor(myRobot.kRearRightMotor, true);
		// Initiate AxisCamera
		AxisCamera &robocam = AxisCamera::GetInstance();
		// Await Initialization
		Wait(0.01);
		// Initialize Limit Switches
		DigitalInput armBaseLimitBack(1);
		DigitalInput armBaseLimitFront(2);
		DigitalInput armTopLimitBack(3);
		DigitalInput armTopLimitFront(4);
		// Temporary Holding Values For Arm Inputs
		float currentArmTop;
		float currentArmBase;
		// Start Operator Control Loop
		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			// Refresh camera
			CameraRefresh(robocam);
			myRobot.MecanumDrive_Cartesian(baseStick1.GetX(), baseStick1.GetY(), baseStick2.GetX(), 0);
			// Control for the arm
			currentArmTop = armStick.GetY();
			if (armBaseLimitBack.Get() > 0){
				if (currentArmTop > 0) {
					armMotorTop.SetSpeed(currentArmTop);
				} else {
					armMotorTop.SetSpeed(.5);
				}
			} else if (armBaseLimitFront.Get() > 0){
				if (currentArmTop < 0) {
					armMotorTop.SetSpeed(currentArmTop);
				} else {
					armMotorTop.SetSpeed(-.5);
				}
			} else {
					armMotorTop.SetSpeed(currentArmTop/10.0);
			}
			// Control for the arm base
			currentArmBase = armStick.GetX();
			if (armBaseLimitBack.Get() == 1){
				if (currentArmBase > 0){
					armMotorBase.SetSpeed(currentArmBase);
				} else {
					armMotorBase.SetSpeed(.5);
				}
			} else if (armBaseLimitFront.Get() == 1){
				if (currentArmBase < 0) {
					armMotorBase.SetSpeed(currentArmBase);
				} else {
					armMotorBase.SetSpeed(-.5);
				}
			} else {
				armMotorBase.SetSpeed(currentArmBase/10.0);
			}
			Wait(0.005);// motor update time
		} 
	}
	
	void CameraRefresh(AxisCamera &robocam){
		if (robocam.IsFreshImage()) {
			robocam.GetImage();
		}
	}
};

START_ROBOT_CLASS(Robot);
